


#include "user_serialization.h"



/* This function convert structure control_pid_est_t to array of uint8_t and calculate controll summ. */
void user_serialisation_api(uint8_t *msg,control_pid_est_t *board, uint16_t max_length)
{
    uint16_t current_position=0;
    uint16_t current_CRC;

#if OLD_BLOCKED
    uint16_t i=0;
#endif
    current_position= serialise8(msg, 0x0A,current_position,max_length);
    current_position= serialise8(msg, 'D',current_position,max_length);
    current_position= serialise8(msg, 'A',current_position,max_length);
    current_position= serialise8(msg, 'T',current_position,max_length);
    current_position= serialise8(msg, 'A',current_position,max_length);

#if 1
   /* Read "from" write "to" board. */

    /* Horizont. */
    /* Roll dynamic PID.  0*/
    current_position= serialise16(msg, board->PID_Roll_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Roll_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Roll_Kd,current_position,max_length);
    /* Pitch dynamic PID. 6*/
    current_position= serialise16(msg, board->PID_Pitch_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Pitch_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Pitch_Kd,current_position,max_length);
    /* Yaw dynamic PID. 12*/
    current_position= serialise16(msg, board->PID_Yaw_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Yaw_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Yaw_Kd,current_position,max_length);

    /* Used for ordinare PID. 18*/
    current_position= serialise16(msg, board->PID_Level_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Level_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Level_Kd,current_position,max_length);
    /* Relation in Horrizont Kalman filter between Gyro and Accel 24*/
    current_position= serialise16(msg, board->Horizont_Kalman_Coeff,current_position,max_length);
/* Altitude Baro. 26*/
    /* Used for keep altitude using Baro sensor. */
    current_position= serialise16(msg, board->PID_Altitude_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Altitude_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Altitude_Kd,current_position,max_length);
    /* Used for keep velocity using Baro sensor. 32*/
    current_position= serialise16(msg, board->PID_Velocity_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Velocity_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Velocity_Kd,current_position,max_length);
/* Distance Sonar. */
    /* Used for keep distance using sonar sensor. 38*/
    current_position= serialise16(msg, board->PID_Distance_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Distance_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Distance_Kd,current_position,max_length);
/* Direction. */
    /* Used for keep distance using sonar sensor. 44*/
    current_position= serialise16(msg, board->PID_Direction_Kp,current_position,max_length);
    current_position= serialise16(msg, board->PID_Direction_Ki,current_position,max_length);
    current_position= serialise16(msg, board->PID_Direction_Kd,current_position,max_length);
    /* Relation in Direction Kalman filter between Gyro and Magnetometer 50*/
    current_position= serialise16(msg, board->Direction_Kalman_Coeff,current_position,max_length);
/* PPM zero shift. 52*/
    current_position= serialise16(msg, board->PPM_shift_ch1,current_position,max_length);
    current_position= serialise16(msg, board->PPM_shift_ch2,current_position,max_length);
    current_position= serialise16(msg, board->PPM_shift_ch3,current_position,max_length);
    current_position= serialise16(msg, board->PPM_shift_ch4,current_position,max_length);
    current_position= serialise16(msg, board->PPM_shift_ch5,current_position,max_length);
    current_position= serialise16(msg, board->PPM_shift_ch6,current_position,max_length);
    /* Read only from board. 64*/
   /*    int16    */
/* Acceleration. */
    current_position= serialise16(msg, board->Acceleration_raw_X,current_position,max_length);
    current_position= serialise16(msg, board->Acceleration_raw_Y,current_position,max_length);
    current_position= serialise16(msg, board->Acceleration_raw_Z,current_position,max_length);
    current_position= serialise16(msg, board->Acceleration_Filtered_X,current_position,max_length);
    current_position= serialise16(msg, board->Acceleration_Filtered_Y,current_position,max_length);
    current_position= serialise16(msg, board->Acceleration_Filtered_Z,current_position,max_length);
/* Gyro. 76*/
    current_position= serialise16(msg, board->Gyro_raw_XY,current_position,max_length);
    current_position= serialise16(msg, board->Gyro_raw_YZ,current_position,max_length);
    current_position= serialise16(msg, board->Gyro_raw_XZ,current_position,max_length);
    current_position= serialise16(msg, board->Gyro_Filtered_XY,current_position,max_length);
    current_position= serialise16(msg, board->Gyro_Filtered_YZ,current_position,max_length);
    current_position= serialise16(msg, board->Gyro_Filtered_XZ,current_position,max_length);
/* Magnetometer. 88*/
    current_position= serialise16(msg, board->Magnetometer_raw_X,current_position,max_length);
    current_position= serialise16(msg, board->Magnetometer_raw_Y,current_position,max_length);
    current_position= serialise16(msg, board->Magnetometer_raw_Z,current_position,max_length);
    current_position= serialise16(msg, board->Magnetometer_Filtered_X,current_position,max_length);
    current_position= serialise16(msg, board->Magnetometer_Filtered_Y,current_position,max_length);
    current_position= serialise16(msg, board->Magnetometer_Filtered_Z,current_position,max_length);
/* Barometer. 100*/
    current_position= serialise16(msg, board->Barometer_Altitude_raw,current_position,max_length);
    current_position= serialise16(msg, board->Barometer_Altitude_Filtered,current_position,max_length);

    current_position= serialise16(msg, board->Barometer_Velocity_raw,current_position,max_length);
    current_position= serialise16(msg, board->Barometer_Velocity_Filtered,current_position,max_length);
    current_position= serialise16(msg, board->Barometer_Temperature,current_position,max_length);
/* Sonar 110*/
    current_position= serialise16(msg, board->Sonar_Distance,current_position,max_length);
/* Main Rotor rotation speed 112*/
    current_position= serialise32(msg, board->Main_Rotor_Rotation_Speed,current_position,max_length);
/* PWM output. 116*/
    current_position = serialise16(msg, board->Motor_Servo_1,current_position,max_length);
    current_position = serialise16(msg, board->Motor_Servo_2,current_position,max_length);
    current_position = serialise16(msg, board->Motor_Servo_3,current_position,max_length);
    current_position = serialise16(msg, board->Motor_Servo_4,current_position,max_length);
    current_position = serialise16(msg, board->Motor_Servo_5,current_position,max_length);
    current_position = serialise16(msg, board->Motor_Servo_6,current_position,max_length);
/* System data. 128*/
    current_position = serialise32(msg, board->BoardSystemTime,current_position,max_length);
    current_position = serialise32(msg, board->Temp_u32,current_position,max_length);
    current_position = serialise32(msg, board->Temp_i32,current_position,max_length);
/* Control summ. 140*/
    current_CRC =  CRC16_alg( msg+5, current_position-5);//msg+1+4 is start of data block
    current_position = serialise16(msg, current_CRC,current_position,max_length);
    board->CRC_summ  = current_CRC;
#endif
    /* Send array to client. */
#if OLD_BLOCKED
    board_fputc('D');
    board_fputc('A');
    board_fputc('T');
    board_fputc('A');
    for(i=0; i < current_position; i++)
    {
        board_fputc(*msg);
        msg++;
    }
#endif
      DMA_Cmd(DMA1_Channel4, DISABLE);
      DMA_SetCurrDataCounter(DMA1_Channel4, current_position);
      DMA_Cmd(DMA1_Channel4, ENABLE);
     // while(GetStateDMAChannel4()==0) {};
}

/* This function convert structure seansor_raw_data_t to array of uint8_t and calculate of controll summ. */
void sensors_raw_data_serialisation_api(uint8_t *msg,sensor_raw_data_t *board, uint16_t max_length)
{
    uint16_t current_position=0;
    uint16_t current_CRC;
#if OLD_BLOCKED
    uint16_t i=0;
#endif
    // It is some work around about bug? ,but we should send
    // 0x0A( or some byte) for first to have normal serial transmit.
    current_position= serialise8(msg, 0x0A,current_position,max_length);

    // It is start of name of data block
    current_position= serialise8(msg, 'D',current_position,max_length);
    current_position= serialise8(msg, 'A',current_position,max_length);
    current_position= serialise8(msg, 'T',current_position,max_length);
    current_position= serialise8(msg, 'A',current_position,max_length);

    // It is start of data of data  blok
/* Acceleration. 1 - number of byte possition. This is a first byte */
    current_position= serialise16(msg, board->Acceleration_raw_X,current_position,max_length);
    current_position= serialise16(msg, board->Acceleration_raw_Y,current_position,max_length);
    current_position= serialise16(msg, board->Acceleration_raw_Z,current_position,max_length);
/* Gyro. 7*/
    current_position= serialise16(msg, board->Gyro_raw_XY,current_position,max_length);
    current_position= serialise16(msg, board->Gyro_raw_YZ,current_position,max_length);
    current_position= serialise16(msg, board->Gyro_raw_XZ,current_position,max_length);
/* Magnetometer. 13*/
    current_position= serialise16(msg, board->Magnetometer_raw_X,current_position,max_length);
    current_position= serialise16(msg, board->Magnetometer_raw_Y,current_position,max_length);
    current_position= serialise16(msg, board->Magnetometer_raw_Z,current_position,max_length);
/* Quaternions. 19*/
    current_position = serialise32(msg, board->Quaternion[0].u32_value, current_position, max_length);
    current_position = serialise32(msg, board->Quaternion[1].u32_value, current_position, max_length);
    current_position = serialise32(msg, board->Quaternion[2].u32_value, current_position, max_length);
    current_position = serialise32(msg, board->Quaternion[3].u32_value, current_position, max_length);

/* Euler Angles. 35 */
    current_position = serialise32(msg, board->Euler_Angle[0].u32_value, current_position, max_length);
    current_position = serialise32(msg, board->Euler_Angle[1].u32_value, current_position, max_length);
    current_position = serialise32(msg, board->Euler_Angle[2].u32_value, current_position, max_length);

/* Board system time. 47 */
    current_position = serialise32(msg, board->BoardSystemTime,current_position,max_length);

/* Pitch dynamic PID. 51 */
    current_position= serialise16(msg, board->PID->Pitch_kP_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Pitch_kP_pid_dynamic,current_position,max_length);
    current_position= serialise16(msg, board->PID->Pitch_kI_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Pitch_kI_pid_dynamic,current_position,max_length);
    current_position= serialise16(msg, board->PID->Pitch_kD_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Pitch_kD_pid_dynamic,current_position,max_length);

/* Roll dynamic PID. 63 */
    current_position= serialise16(msg, board->PID->Roll_kP_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Roll_kP_pid_dynamic,current_position,max_length);
    current_position= serialise16(msg, board->PID->Roll_kI_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Roll_kI_pid_dynamic,current_position,max_length);
    current_position= serialise16(msg, board->PID->Roll_kD_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Roll_kD_pid_dynamic,current_position,max_length);

/* Yaw dynamic PID. 75 */
    current_position= serialise16(msg, board->PID->Yaw_kP_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Yaw_kP_pid_dynamic,current_position,max_length);
    current_position= serialise16(msg, board->PID->Yaw_kI_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Yaw_kI_pid_dynamic,current_position,max_length);
    current_position= serialise16(msg, board->PID->Yaw_kD_pid,current_position,max_length);
    current_position= serialise16(msg, board->PID->Yaw_kD_pid_dynamic,current_position,max_length);

/* Control summ. 87 */
    /*
    * Control sum calculated from start of data of packet.
    * Header of packet is 5 bytes " 0x0A , 'D','A','T','A' ", so, start address of data of packet is
    * pointer to packet buffer (msg) + 5 , and size of data of packet
    * is size of packet(current_position) - 5.
    */
    current_CRC =  CRC16_alg( msg+5, current_position-5); /*sizeof(msg)+(1+4) for start messages of data block*/
    current_position = serialise16(msg, current_CRC,current_position,max_length); /*current_position now has size of massage packet to send. */

    board->CRC_summ  = current_CRC;

/*  Blocked send array to client. */
/*  Old version. Now we use DMA transfer. */
#if OLD_BLOCKED
    board_fputc('D');
    board_fputc('A');
    board_fputc('T');
    board_fputc('A');
    for(i=0; i < current_position; i++)
    {
        board_fputc(*msg);
        msg++;
    }
#endif

/* New version. Serial TX  by using DMA. */
      DMA_Cmd(DMA1_Channel4, DISABLE);
      DMA_SetCurrDataCounter(DMA1_Channel4, current_position);
      DMA_Cmd(DMA1_Channel4, ENABLE);

 /* Here we can wait before DMS send all bytes.(?need to chect it) */
 /* while(GetStateDMAChannel4()==0) {}; */
}





























/* serialisation of 8bits value. */
uint8_t serialise8(uint8_t *byte_pointer, uint8_t input_value, uint16_t current_position,uint16_t max_length)
{
    if(current_position < max_length)
    {
        byte_pointer  = byte_pointer + current_position;
        *byte_pointer = (uint8_t)input_value;
        current_position++;
        return current_position;
    }
    return 0;
}




/* serialisation of 16bits value. LSB is fist byte. */
uint16_t serialise16(uint8_t *byte_pointer, uint16_t input_value, uint16_t current_position,uint16_t max_length)
{
    if(current_position < (max_length-1))//(max_lenght-1) means that at least 2 byte is free
    {
        byte_pointer = byte_pointer + current_position;
        /* LSB */
        *byte_pointer = (uint8_t)input_value;
        /* MSB */
        byte_pointer++;
        *byte_pointer = (uint8_t)(input_value >> 8);
        current_position = current_position+2;
        return current_position;
    }
    return 0;
}

/* serialisation of 32bits value. LSB is fist byte. */
uint16_t serialise32(uint8_t *byte_pointer, uint32_t input_value, uint16_t current_position,uint16_t max_length)
{
    if(current_position < (max_length-3))//(max_lenght-3) means that at least 4 byte is free
    {
        byte_pointer = byte_pointer + current_position;
        /* LSB 0x000000FF */
       *byte_pointer = (uint8_t)input_value;
        /*     0x0000FF00 */
        byte_pointer++;
       *byte_pointer = (uint8_t)(input_value >> 8);
        /*     0x00FF0000 */
        byte_pointer++;
       *byte_pointer = (uint8_t)(input_value >> 16);
        /* MSB 0xFF000000 */
        byte_pointer++;
       *byte_pointer = (uint8_t)(input_value >> 24);

        current_position = current_position+4;
        return current_position;
    }
    return 0;
}











